/* * Copyright (c) 2021 BlackWalnut Labs., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include <stdio.h>
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_watchdog.h"
#include "iot_i2c.h"
#include "iot_io.h"
#include "iot_errno.h"
#include "icm20948.h"

const uint8_t I2C_PORT_IDX = 0;
const uint32_t I2C_BAUDRATE = 400000;

ICM_20948_Status_e serif_i2c_write(uint8_t regaddr, uint8_t *pdata, uint32_t len, void *user)
{
    if (user == NULL)
    {
        return ICM_20948_Stat_ParamErr;
    }

    uint8_t addr = ((ICM_20948 *)user)->_addr;
    uint8_t send_data[1 + len];
    send_data[0] = regaddr;
    memcpy(&send_data[1], pdata, len);

    unsigned int retval = IoTI2cWrite(I2C_PORT_IDX, (addr << 1) | 0x0, send_data, len + 1);
    return retval == IOT_SUCCESS ? ICM_20948_Stat_Ok : ICM_20948_Stat_NoData;
}

ICM_20948_Status_e serif_i2c_read(uint8_t reg, uint8_t *buff, uint32_t len, void *user)
{
    if (user == NULL)
    {
        return ICM_20948_Stat_ParamErr;
    }

    uint8_t addr = ((ICM_20948 *)user)->_addr;
    // uint8_t send_data[1] = {reg};
    unsigned int retval = IoTI2cWriteRead(I2C_PORT_IDX, (addr << 1) | 0x0, &reg, 1, buff, len);

    return retval == IOT_SUCCESS ? ICM_20948_Stat_Ok : ICM_20948_Stat_NoData;
}

void printScaledAGMT(ICM_20948 *sensor)
{
    printf("Scaled. Acc (mg) [ ");
    printf("%f", accX(sensor));
    printf(", ");
    printf("%f", accY(sensor));
    printf(", ");
    printf("%f", accZ(sensor));
    printf(" ], Gyr (DPS) [ ");
    printf("%f", gyrX(sensor));
    printf(", ");
    printf("%f", gyrY(sensor));
    printf(", ");
    printf("%f", gyrZ(sensor));
    printf(" ], Mag (uT) [ ");
    printf("%f", magX(sensor));
    printf(", ");
    printf("%f", magY(sensor));
    printf(", ");
    printf("%f", magZ(sensor));
    printf(" ], Tmp (C) [ ");
    printf("%f", temperature(sensor));
    printf(" ]");
    printf("\r\n");
}

static void ICM20948_STARTUP(void)
{
    // Initial I2C
    IoTI2cInit(I2C_PORT_IDX, I2C_BAUDRATE);
    IoTIoSetFunc(IOT_IO_NAME_10, IOT_IO_FUNC_10_I2C0_SCL);
    IoTIoSetFunc(IOT_IO_NAME_9, IOT_IO_FUNC_9_I2C0_SDA);

    ICM_20948 icm_20948;
    ICM_20948_Device_t device = {0};
    ICM_20948_Serif_t serif = {0};
    serif.write = serif_i2c_write;
    serif.read = serif_i2c_read;
    serif.user = &icm_20948;
    device._serif = &serif;
    icm_20948._addr = ICM_20948_I2C_ADDR_AD0;
    icm_20948._device = device;

    ICM_20948_Status_e retval = icm20948_begin(&icm_20948);

    while (true)
    {
        if (dataReady(&icm_20948))
        {
            getAGMT(&icm_20948);         // The values are only updated when you call 'getAGMT'
            printScaledAGMT(&icm_20948); // This function takes into account the scale settings from when the measurement was made to calculate the values with units
            osDelay(30);
        }
        else
        {
            printf("Waiting for data\r\n");
            osDelay(500);
        }
    }
}

static void APP(void)
{
    IoTWatchDogDisable();

    osThreadAttr_t attr;

    attr.name = "ICM20948_STARTUP";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 10240;
    attr.priority = osPriorityLow1;

    if (osThreadNew(ICM20948_STARTUP, NULL, &attr) == NULL)
    {
        printf("[ICM20948_STARTUP] Falied to create ICM20948_STARTUP!\n");
    }
}

SYS_RUN(APP);